#!/usr/bin/env python  
"""
My solution for Assignment 2, Part 2, Task 3 in COMP 4766, Winter 2014.

Name: Daniel Cook
Email: dac456@mun.ca
Student number: 
"""

from geometry_msgs.msg import Twist
import math
import rospy
import tf
from tf.transformations import euler_from_quaternion

class SmoothController1TF:
    def __init__(self, listener, goalIx, goalIy):
        """
        Initialize the class with the position of the goal in the robot ref.
        frame (goalIx, goalIy).
        """
        
        self.listener = listener
        self.goalIx = goalIx
        self.goalIy = goalIy
        
        self.br = tf.TransformBroadcaster()

    def get_twist(self):
        """
        Given the current pose of the robot (x, y, theta) compute and return
        the appropriate Twist message.
        """
        
        # broadcast the goal position relative to the odom frame
        self.br.sendTransform((self.goalIx, self.goalIy, 0.0), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "goal", "odom")
        
        t = Twist()
        t.linear.x = 0
        t.angular.z = 0
        
        try:
            # make sure the goal frame exists
            self.listener.waitForTransform('/base_footprint', '/goal', rospy.Time(), rospy.Duration(1.0))
            
            # transform the goal frame relative to base_footprint
            (trans,rot) = self.listener.lookupTransform('/base_footprint', '/goal', rospy.Time(0))
            
            x = trans[0]
            y = trans[1]
            euler_angles = euler_from_quaternion(rot)
            theta = euler_angles[2]
            
            v = 1*x
            w = 1*y
            
            if(x != 0):
                t.linear.x = v
                
            if(y >= 0):
                t.angular.z = -w
            else:
                t.angular.z = w
                
        except Exception, e:
            print(e)
            
        return t
